#ifndef NAVIGATIONCONTROLLER_H
#define NAVIGATIONCONTROLLER_H

#include "Params.h"
#include <geometry_msgs/PoseStamped.h>
#include <cmath>
#include <geometry_msgs/Twist.h>
#include "std_msgs/String.h"
#define TOLERANCE 0.05f
typedef struct _ACTION_DATA_ {
    float v;
    float w;
}ACTION_DATA_S;

class NavigationController {
public:
    // 构造函数，参数为params

    NavigationController(std::shared_ptr<Params>& params_);
    void PublishGoalCmd(float x, float y, float z);
    void PublishMotorCmd(float v, float w);
    bool DockingTaskAction_WithPCL(ACTION_DATA_S* action, const Point2D_ &g_pose_2d, const Point2D_ &g_cross_point_) ;
    // 角度归一化
    double NormalizeAngle(double angle);
private:
    ros::NodeHandle nh;
    ros::Publisher pub_goal_;
    ros::Publisher cmd_vel_;
    // 定义params
    std::shared_ptr<Params> params;
};

#endif // NAVIGATIONCONTROLLER_H